#!/usr/bin/env python

import rospy
import sys
from math import pi
import moveit_commander
from geometry_msgs.msg import PoseStamped, Pose
import tf
from tf.transformations import *

class MoveItIkDemo:
	def __init__(self):
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node("moveit_ik_demo")
		arm = moveit_commander.MoveGroupCommander("xarm")
		end_effector_link = arm.get_end_effector_link()
		rospy.loginfo("end_effector_link is : "+end_effector_link)
		reference_frame = "base_link"
		arm.set_pose_reference_frame(reference_frame)
		arm.allow_replanning(True)
		arm.set_goal_position_tolerance(0.02)
		arm.set_goal_orientation_tolerance(0.03)
		arm.set_max_velocity_scaling_factor(1.0)
		arm.set_max_acceleration_scaling_factor(0.6)
		
		arm.set_named_target("Home")
		arm.go()
		rospy.sleep(1)
		
		target_pose = PoseStamped()
		target_pose.header.frame_id = reference_frame
		target_pose.header.stamp = rospy.Time.now()
		target_pose.pose.position.x = 0.3
		target_pose.pose.position.y = 0.1
		target_pose.pose.position.z = 0.25
		quaternion = tf.transformations.quaternion_from_euler(0, pi/2, 0)
		rospy.loginfo("Quaternion is : ")
		rospy.loginfo(quaternion)
		target_pose.pose.orientation.x = quaternion[0]
		target_pose.pose.orientation.y = quaternion[1]
		target_pose.pose.orientation.z = quaternion[2]
		target_pose.pose.orientation.w = quaternion[3]
		arm.set_start_state_to_current_state()
		arm.set_pose_target(target_pose, end_effector_link)
		plan_success, traj, planning_time, error_code  = arm.plan()
		arm.execute(traj)
		rospy.sleep(1)

		current_pose = arm.get_current_pose(end_effector_link)
		rospy.loginfo("Current pose is :")
		rospy.loginfo(current_pose)
		
		current_joint_positions = arm.get_current_joint_values()
		rospy.loginfo("Current joint positions is :")
		rospy.loginfo(current_joint_positions)
		
		arm.shift_pose_target(0, 0.05, end_effector_link)
		plan_success, traj, planning_time, error_code  = arm.plan()
		arm.execute(traj)
		rospy.sleep(1)
	
		moveit_commander.roscpp_shutdown()
		moveit_commander.os._exit(0)


if __name__ == "__main__":
	MoveItIkDemo()